#!/usr/bin/env python3
# coding=utf-8

import sys
import re
import rospy
from xfultrasonicdriver.msg import ultrasonic_msg
from std_msgs.msg import String


# from matplotlib import pyplot as plt
import numpy as np

# 创建实时绘制横纵轴变量
xdata, ydata = [], []  # 用于保存绘图数据，最开始时什么都没有，默认为空



# def callback(msg):
#     dis = msg.psd01
#     x_idx=x_idx+1
#     xdata.append(x_idx)	# 添加i到x轴的数据中
#     ydata.append(dis)	# 添加i的平方到y轴的数据中


def plot_it():
    rospy.init_node("plot_ulrasound", anonymous=True)  
    # rospy.Subscriber("/psd_ultrasonic",ultrasonic_msg,callback)

    # plt.ion() #开启interactive mode 成功的关键函数
    # figure, ax = plt.subplots(figsize=(8,8))
    # # fig = plt.figure(figsize=(10, 5))  # 创建图
    # plt.ylim(0, 5.0)  # Y轴取值范围
    # # plt.xlim(-1, 1.5)  # X轴取值范围
    # plt.title("ulstrasound")   # 标题
    # plt.xlabel("X")  # X轴标签
    # plt.ylabel("D")  # Y轴标签
  

    rate = rospy.Rate(100) # 20:20hz
    count = 0
    x_idx = 0
    try:

        while not rospy.is_shutdown():
            msg = rospy.wait_for_message("/psd_ultrasonic", ultrasonic_msg, timeout=None)
            x_idx = x_idx + 1
            # xdata.append(x_idx)
            # ydata.append(msg.psd03)
            
            if msg.psd03<0xFE:
                rospy.loginfo("%2d ", msg.psd03)

            count+=1
            if count==10:
                count = 0
                # plt.plot(xdata, ydata, "r--",lw=0.5)  # 绘制折线图
                # plt.plot(xdata, ydata, ".",markersize=0.1,color='b')  # 绘制折线图
                # plt.draw()#注意此函数需要调用
                # figure.canvas.flush_events()

            rate.sleep()

    except Exception as e:
        rospy.loginfo("ex:%s",e)
        pass

    # rospy.loginfo("X min,max= %.2f,%.2f; Y min,max=  %.2f,%.2f",min(xdata),max(xdata),min(ydata),max(ydata))    
    # plt.savefig('mcar1.png', dpi=300, bbox_inches='tight')
    # rospy.loginfo("plt saved to mcar1.png")


if __name__ == '__main__':
    plot_it()
